#include "OpencvPreprocess.h"
#include <vector>

cv::Mat preprocess(cv::Mat origin_img, std::vector<cv::Point2f> &centers)
{
    // Convert to HSV
    cv::Mat HSV;
    cv::cvtColor(origin_img, HSV, cv::COLOR_BGR2HSV);

    // Define white color range in HSV
    cv::Scalar lowerColor(20, 160, 160);
    cv::Scalar upperColor(30, 180, 185);

    // Extract white regions
    cv::Mat binary;
    cv::inRange(HSV, lowerColor, upperColor, binary);

    // Apply median blur to remove noise
    cv::medianBlur(binary, binary, 9);

    // Show binary image
    cv::imshow("median", binary);

    // Find contours
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(binary, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

    // Draw contours on the original image
    cv::Mat res = origin_img.clone();
    cv::drawContours(res, contours, -1, cv::Scalar(0, 0, 255), 4);
    // cv::imshow("res", res);

    // Print and mark center points on the original image
    int L = contours.size();
    // std::cout << "coordinate:" << std::endl;

    for (int i = 0; i < L; ++i) {
        cv::Point2f center;
        float radius;
        cv::minEnclosingCircle(contours[i], center, radius);
        centers.push_back(center);

        // Draw center point
        cv::circle(origin_img, center, 3, cv::Scalar(0, 0, 255), 5);

        // Print center coordinates
        // std::cout << "Center: (" << center.x << ", " << center.y << ")" << std::endl;
    }

    // Show the image with marked center points
    cv::imshow("img2", origin_img);

    return origin_img;
}